#include "protocol/modbus/ModbusRTU.h"
#include "core/Logger.h"

#include <modbus.h>
#include <cerrno>
#include <cstring>

namespace ems {
namespace protocol {

ModbusRTU::ModbusRTU(const std::string& device, int baudRate, char parity, int dataBits, int stopBits)
    : device_(device), baudRate_(baudRate), parity_(parity), dataBits_(dataBits), stopBits_(stopBits),
      slaveId_(1), timeout_(3000), ctx_(nullptr), connected_(false) {
}

ModbusRTU::~ModbusRTU() {
    disconnect();
}

bool ModbusRTU::connect() {
    if (connected_) {
        return true;
    }
    
    ctx_ = modbus_new_rtu(device_.c_str(), baudRate_, parity_, dataBits_, stopBits_);
    if (!ctx_) {
        updateLastError();
        LOG_ERROR("Failed to create Modbus RTU context: " << lastError_);
        return false;
    }
    
    if (modbus_set_slave(ctx_, slaveId_) == -1) {
        updateLastError();
        LOG_ERROR("Failed to set Modbus slave ID: " << lastError_);
        modbus_free(ctx_);
        ctx_ = nullptr;
        return false;
    }
    
    struct timeval timeout;
    timeout.tv_sec = timeout_ / 1000;
    timeout.tv_usec = (timeout_ % 1000) * 1000;
    
    if (modbus_set_response_timeout(ctx_, timeout.tv_sec, timeout.tv_usec) == -1) {
        updateLastError();
        LOG_WARN("Failed to set response timeout: " << lastError_);
    }
    
    if (modbus_connect(ctx_) == -1) {
        updateLastError();
        LOG_ERROR("Failed to connect to Modbus RTU device: " << lastError_);
        modbus_free(ctx_);
        ctx_ = nullptr;
        return false;
    }
    
    connected_ = true;
    LOG_INFO("Successfully connected to Modbus RTU device: " << device_);
    return true;
}

void ModbusRTU::disconnect() {
    if (ctx_) {
        if (connected_) {
            modbus_close(ctx_);
            connected_ = false;
        }
        modbus_free(ctx_);
        ctx_ = nullptr;
        LOG_INFO("Disconnected from Modbus RTU device");
    }
}

bool ModbusRTU::isConnected() const {
    return connected_ && ctx_ != nullptr;
}

void ModbusRTU::setSlaveId(int slaveId) {
    slaveId_ = slaveId;
    if (ctx_) {
        modbus_set_slave(ctx_, slaveId_);
    }
}

void ModbusRTU::setTimeout(int timeoutMs) {
    timeout_ = timeoutMs;
    if (ctx_) {
        struct timeval timeout;
        timeout.tv_sec = timeoutMs / 1000;
        timeout.tv_usec = (timeoutMs % 1000) * 1000;
        modbus_set_response_timeout(ctx_, timeout.tv_sec, timeout.tv_usec);
    }
}

bool ModbusRTU::readHoldingRegisters(uint16_t address, uint16_t count, std::vector<uint16_t>& data) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    data.resize(count);
    int result = modbus_read_registers(ctx_, address, count, data.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to read holding registers: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusRTU::readInputRegisters(uint16_t address, uint16_t count, std::vector<uint16_t>& data) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    data.resize(count);
    int result = modbus_read_input_registers(ctx_, address, count, data.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to read input registers: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusRTU::readCoils(uint16_t address, uint16_t count, std::vector<bool>& data) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    std::vector<uint8_t> rawData(count);
    int result = modbus_read_bits(ctx_, address, count, rawData.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to read coils: " << lastError_);
        return false;
    }
    
    data.resize(count);
    for (size_t i = 0; i < count; ++i) {
        data[i] = rawData[i] != 0;
    }
    
    return true;
}

bool ModbusRTU::readDiscreteInputs(uint16_t address, uint16_t count, std::vector<bool>& data) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    std::vector<uint8_t> rawData(count);
    int result = modbus_read_input_bits(ctx_, address, count, rawData.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to read discrete inputs: " << lastError_);
        return false;
    }
    
    data.resize(count);
    for (size_t i = 0; i < count; ++i) {
        data[i] = rawData[i] != 0;
    }
    
    return true;
}

bool ModbusRTU::writeSingleRegister(uint16_t address, uint16_t value) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    int result = modbus_write_register(ctx_, address, value);
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to write single register: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusRTU::writeMultipleRegisters(uint16_t address, const std::vector<uint16_t>& values) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    int result = modbus_write_registers(ctx_, address, values.size(), values.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to write multiple registers: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusRTU::writeSingleCoil(uint16_t address, bool value) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    int result = modbus_write_bit(ctx_, address, value ? 1 : 0);
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to write single coil: " << lastError_);
        return false;
    }
    
    return true;
}

bool ModbusRTU::writeMultipleCoils(uint16_t address, const std::vector<bool>& values) {
    if (!isConnected()) {
        lastError_ = "Not connected";
        return false;
    }
    
    std::vector<uint8_t> rawData(values.size());
    for (size_t i = 0; i < values.size(); ++i) {
        rawData[i] = values[i] ? 1 : 0;
    }
    
    int result = modbus_write_bits(ctx_, address, values.size(), rawData.data());
    if (result == -1) {
        updateLastError();
        LOG_ERROR("Failed to write multiple coils: " << lastError_);
        return false;
    }
    
    return true;
}

std::string ModbusRTU::getLastError() const {
    return lastError_;
}

void ModbusRTU::updateLastError() const {
    lastError_ = std::string(modbus_strerror(errno));
}

}  // namespace protocol
}  // namespace ems